/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : imu_bias_factor.h
 *@brief   : imu bias factor
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/27
 *
 */

#ifndef IMU_BIAS_FACTOR_H
#define IMU_BIAS_FACTOR_H

#include <ceres/ceres.h>

#include "preintegration/imu_preintegration.h"

#include "common/state.h"

namespace optimizer {

using namespace preintegration;

class ImuBiasFactor : public ceres::SizedCostFunction<3, 3, 3>{
public:  
   ImuBiasFactor() {
      cov_ = Eigen::Matrix<double, 3, 3>::Identity();
      cov_(0,0) = 0.005; // bg
      cov_(1,1) = 0.01;  // bax
      cov_(2,2) = 0.01;  // bay
   }

   // ceres-solver
   // parameters[0] : bg, ba
   virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {

      Eigen::Vector3d bias_i(parameters[0][0], parameters[0][1], parameters[0][2]);
      Eigen::Vector3d bias_j(parameters[1][0], parameters[1][1], parameters[1][2]);

      Eigen::Map<Eigen::Matrix<double, 3, 1>> residual(residuals);


      residual = bias_j - bias_i - Eigen::Vector3d::Zero();

      // LOG(INFO) << "bias_i " << bias_i.transpose() << ", bias_j : " << bias_j.transpose() << std::endl;
      // LOG(INFO) << "residual " << residual.transpose() << std::endl;

      Eigen::Matrix<double, 3, 3> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 3, 3>>(cov_.inverse()).matrixL().transpose();
      residual = sqrt_info * residual;

      if (jacobians) {
         if (jacobians[0]) { // dRi 
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacobianBias_i(jacobians[0]);
            jacobianBias_i.setIdentity(); 

            jacobianBias_i = -sqrt_info * jacobianBias_i;
         }
         if (jacobians[1]) { // dvi
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacobianBias_j(jacobians[1]);
            jacobianBias_j.setIdentity(); 

            jacobianBias_j = sqrt_info * jacobianBias_j;
         }
      }
   }

private:
   Eigen::Matrix<double, 3, 3> cov_;
};

}

#endif 
